/*
  TB6612.cpp - Libreria per gestire i movimenti di un motore DC con il modulo TB6612
  Autore:   Andrea Lombardo
  Sito web: http://www.lombardoandrea.com  
*/

#include "TB6612.h"

typedef void (*CallBackFunction)();

TB6612::TB6612(uint8_t pinEnable, uint8_t pinIN1, uint8_t pinIN2) {
    _pinEnable = pinEnable;
    _pinIN1 = pinIN1;
    _pinIN2 = pinIN2;
    _pwmVal = 100;
    _isMoving = false;
    _canMove = true;
    _lastMs = 0;
    _direction = STOP;

    pinMode(_pinEnable, OUTPUT);
    pinMode(_pinIN1, OUTPUT);
    pinMode(_pinIN2, OUTPUT);
    digitalWrite(_pinEnable, LOW);
    digitalWrite(_pinIN1, LOW);
    digitalWrite(_pinIN2, LOW);
}

TB6612::TB6612(uint8_t pinIN1, uint8_t pinIN2) {
    _pinEnable = -1;
    _pinIN1 = pinIN1;
    _pinIN2 = pinIN2;
    _pwmVal = 255; // It's always at the max speed due to jumper on module
    _isMoving = false;
    _canMove = true;
    _lastMs = 0;
    _direction = STOP;

    pinMode(_pinIN1, OUTPUT);
    pinMode(_pinIN2, OUTPUT);
}

void TB6612::setSpeed(unsigned short pwmVal) {
    _pwmVal = pwmVal;
}

unsigned short TB6612::getSpeed() {
    return _pwmVal;
}

void TB6612::forward() {
    digitalWrite(_pinIN1, HIGH);
    digitalWrite(_pinIN2, LOW);

    analogWrite(_pinEnable, _pwmVal);

    _direction = FORWARD;
    _isMoving = true;
}

void TB6612::backward() {
    digitalWrite(_pinIN1, LOW);
    digitalWrite(_pinIN2, HIGH);

    analogWrite(_pinEnable, _pwmVal);

    _direction = BACKWARD;
    _isMoving = true;
}

void TB6612::run(TB6612::Direction direction) {
    switch (direction) {
        case BACKWARD:
            this->backward();
            break;
        case FORWARD:
            this->forward();
            break;
        case STOP:
            this->stop();
            break;
    }
}

//Timing and callback

void TB6612::runFor(unsigned long delay, TB6612::Direction direction, CallBackFunction callback) {

    if ((_lastMs == 0) && _canMove) {
        _lastMs = millis();

        switch (direction) {
            case FORWARD:
                this->forward();
                break;
            case BACKWARD:
                this->backward();
                break;
            case STOP:
            default:
                this->stop();
                break;
        }
    }

    if (((millis() - _lastMs) > delay) && _canMove) {
        this->stop();
        _lastMs = 0;
        _canMove = false;

        callback();
    }
}

void TB6612::runFor(unsigned long delay, TB6612::Direction direction) {
    this->runFor(delay, direction, fakeCallback);
}

void TB6612::forwardFor(unsigned long delay, CallBackFunction callback) {
    this->runFor(delay, FORWARD, callback);
}

void TB6612::forwardFor(unsigned long delay) {
    this->runFor(delay, FORWARD);
}

void TB6612::backwardFor(unsigned long delay, CallBackFunction callback) {
    this->runFor(delay, BACKWARD, callback);
}

void TB6612::backwardFor(unsigned long delay) {
    this->runFor(delay, BACKWARD);
}

void TB6612::stop() {
    digitalWrite(_pinIN1, LOW);
    digitalWrite(_pinIN2, LOW);

    analogWrite(_pinEnable, 255);

    _direction = STOP;
    _isMoving = false;
}

void TB6612::reset() {
    _canMove = true;
}

boolean TB6612::isMoving() {
    return _isMoving;
}

TB6612::Direction TB6612::getDirection() {
    return _direction;
}

void TB6612::fakeCallback() {
}